#
import math
import numpy as np
from scipy.stats import ncx2
from scipy.special import erfc
from scipy.special import gammainc

class RadarDetection(object):
    def __init__(self):
        self.name = 'core.radar_detection.RadarDetection'

    @staticmethod
    def que_func(x):
        """
        计算Q函数的值（标准正态分布的右尾概率）
        使用近似公式：
            x >= 0: Eq.(2.17) and (2.18)
            x < 0 : 利用Q(-x) = 1 - Q(x)的性质
        
        参数:
            x : 输入值（标量或数组）
        
        返回:
            fofx : Q函数值
        """
        # 处理标量和数组输入
        x = np.asarray(x)
        result = np.zeros_like(x)
        
        # 处理x >= 0的情况
        mask_non_neg = x >= 0
        if np.any(mask_non_neg):
            x_non_neg = x[mask_non_neg]
            denom = 0.661 * x_non_neg + 0.339 * np.sqrt(x_non_neg**2 + 5.51)
            expo = np.exp(-x_non_neg**2 / 2.0)
            result[mask_non_neg] = 1.0 - (1.0 / np.sqrt(2.0 * np.pi)) * (1.0 / denom) * expo
        
        # 处理x < 0的情况（使用Q(-x) = 1 - Q(x)）
        mask_neg = x < 0
        if np.any(mask_neg):
            result[mask_neg] = 1.0 - RadarDetection.que_func(-x[mask_neg])
        
        return result.item() if np.isscalar(x) else result
    
    @staticmethod
    def marcumsq_new(a, b):
        df = 2  # 自由度，对应m=1
        nc = a ** 2  # 非中心参数
        x = b ** 2  # 积分上限
        return 1 - ncx2.cdf(x, df, nc)
    
    def marcumsq(a, b):
        """
        使用Parl方法计算Marcum Q函数
        
        参数:
            a : 非中心参数 (标量)
            b : 检测阈值 (标量)
            
        返回:
            Pd : 检测概率
        """
        max_test_value = 5000.0
        
        if a < b:
            alphan0 = 1.0
            dn = a / b
        else:
            alphan0 = 0.0
            dn = b / a
        
        alphan_1 = 0.0
        betan0 = 0.5
        betan_1 = 0.0
        D1 = dn
        n = 0
        ratio = 2.0 / (a * b)
        r1 = 0.0
        betan = 0.0
        alphan = 0.0
        
        # 迭代直到betan足够大
        while betan < 1000.0:
            n += 1
            alphan = dn + ratio * n * alphan0 + alphan_1
            betan = 1.0 + ratio * n * betan0 + betan_1
            
            alphan_1 = alphan0
            alphan0 = alphan
            betan_1 = betan0
            betan0 = betan
            dn = dn * D1
            
            # 安全措施：防止无限循环
            if n > max_test_value:
                break
        
        # 计算初步概率值
        PD = (alphan0 / (2.0 * betan0)) * np.exp(-(a - b)**2 / 2.0)
        
        # 根据a和b的关系调整结果
        if a >= b:
            PD = 1.0 - PD
        
        return PD
    
    @staticmethod
    def improv_fac(n_P, pfa, pd):
        """
        Computes the non-coherent integration improvement factor
        using the empirical formula defined in Eq. (2.49)
        """
        fact1 = 1.0 + np.log10(1.0 / pfa) / 46.6
        fact2 = 6.79 * (1.0 + 0.235 * pd)
        fact3 = 1.0 - 0.14 * np.log10(n_P) + 0.0183 * (np.log10(n_P))**2
        return fact1 * fact2 * fact3 * np.log10(n_P)
    
    @staticmethod
    def factor(n0):
        n = n0 + 9.0
        n2 = n * n
        # Stirling's approximation with correction terms
        temp = (n-1) * math.log(n) - n + math.log(math.sqrt(2.0 * math.pi * n)) + \
            ((1.0 - (1.0/30.0 + (1.0/105.0)/n2) / n2) / 12.0) / n
        # Subtract log of the product: (n-1)*(n-2)*...*(n-8)
        log_prod = 0.0
        for k in range(1, 9):
            term = n - k
            log_prod += math.log(term)
        val = temp - log_prod
        return val

    @staticmethod
    def incomplete_gamma(vt, np_val):
        if np_val == 1:
            value1 = vt * math.exp(-vt)
            value = 1.0 - math.exp(-vt)
            return value
        
        eps = 1.000000001
        sumold = 1.0
        sumnew = 1.0
        calc1 = 1.0
        calc2 = np_val
        tiny = 1e-10  # Small constant to avoid log(0) and division by zero
        vt_adj = vt + tiny
        xx = np_val * math.log(vt_adj) - vt - RadarDetection.factor(calc2)
        temp1 = math.exp(xx)
        temp2 = np_val / vt_adj
        
        if vt >= np_val:
            diff = 0.0
            ratio = 1000.0
            while ratio >= eps:
                diff += 1.0
                calc1 = calc1 * (calc2 - diff) / vt_adj
                sumnew = sumold + calc1
                ratio = sumnew / sumold
                sumold = sumnew
            value = 1.0 - temp1 * sumnew * temp2
            return value
        else:
            diff = 0.0
            ratio = 1000.0
            while ratio >= eps:
                diff += 1.0
                calc1 = calc1 * vt_adj / (calc2 + diff)
                sumnew = sumold + calc1
                ratio = sumnew / sumold
                sumold = sumnew
            value = temp1 * sumnew
            return value
        
    @staticmethod
    def threshold(nfa, np_val):
        """计算雷达检测阈值"""
        eps = 1e-9
        max_iter = 1000  # 最大迭代次数防止无限循环
        pfa = np_val * math.log(2) / nfa
        sqrtpfa = math.sqrt(-math.log10(pfa))
        sqrtnp = math.sqrt(np_val)
        
        # 初始阈值估计
        vt0 = np_val - sqrtnp + 2.3 * sqrtpfa * (sqrtpfa + sqrtnp - 1.0)
        vt = vt0
        delta = 10000.0
        iteration = 0
        
        while abs(delta) >= vt0 and iteration < max_iter:
            iteration += 1
            igf = RadarDetection.incomplete_gamma(vt0, np_val)
            num = 0.5 ** (np_val / nfa) - igf
            
            # 计算分母：exp((np-1)*log(vt0) - vt0) / (np-1)!
            temp = (np_val - 1) * math.log(vt0 + eps) - vt0 - RadarDetection.factor(np_val - 1)
            deno = math.exp(temp)
            
            # 牛顿-拉夫森更新
            vt = vt0 + num / (deno + eps)
            delta = abs(vt - vt0) * 10000.0
            vt0 = vt
        
        return pfa, vt
    
    @staticmethod
    def pd_swerling5(input1, indicator, np_val, snrbar_db):
        """
        计算Swerling V或0类目标的检测概率
        
        参数:
            input1: 虚警概率(Pfa)或虚警数(Nfa)
            indicator: 1表示input1是Pfa，否则是Nfa
            np_val: 脉冲累积数
            snrbar_db: 信噪比(dB)
        
        返回:
            检测概率(Pd)
        """
        if np_val == 1:
            raise ValueError("np must be greater than 1")
        
        # 转换信噪比为线性值
        snrbar = 10.0 ** (snrbar_db / 10.0)
        eps = 1e-9
        delmax = 1e-5
        
        # 计算阈值Vt
        if indicator != 1:
            nfa = input1
            pfa = np_val * math.log(2) / nfa
        else:
            pfa = input1
            nfa = np_val * math.log(2) / pfa
        
        sqrtpfa = math.sqrt(-math.log10(pfa))
        sqrtnp = math.sqrt(np_val)
        vt0 = np_val - sqrtnp + 2.3 * sqrtpfa * (sqrtpfa + sqrtnp - 1.0)
        vt = vt0
        delta = 10000.0
        max_iter = 1000
        iter_count = 0
        
        # 牛顿-拉夫森迭代求阈值
        while abs(delta) >= vt0 and iter_count < max_iter:
            iter_count += 1
            igf = RadarDetection.incomplete_gamma(vt0, np_val)
            num = 0.5 ** (np_val / nfa) - igf
            
            # 计算分母
            temp = (np_val - 1) * math.log(vt0 + eps) - vt0 - RadarDetection.factor(np_val - 1)
            deno = math.exp(temp)
            
            # 更新阈值
            vt = vt0 + num / (deno + eps)
            delta = abs(vt - vt0) * 10000.0
            vt0 = vt
        
        # 计算Gram-Charlier级数系数
        temp1 = 2.0 * snrbar + 1.0
        omegabar = math.sqrt(np_val * temp1)
        c3 = -(snrbar + 1.0 / 3.0) / (math.sqrt(np_val) * temp1**1.5)
        c4 = (snrbar + 0.25) / (np_val * temp1**2.0)
        c6 = c3 * c3 / 2.0
        
        # 计算归一化变量
        V = (vt - np_val * (1.0 + snrbar)) / omegabar
        Vsqr = V * V
        
        # 计算检测概率
        val1 = math.exp(-Vsqr / 2.0) / math.sqrt(2.0 * math.pi)
        val2 = c3 * (V**2 - 1.0) + c4 * V * (3.0 - V**2) - c6 * V * (V**4 - 10.0 * V**2 + 15.0)
        q = 0.5 * erfc(V / math.sqrt(2.0))
        pd = q - val1 * val2
        
        # 确保检测概率在[0,1]范围内
        return max(0.0, min(1.0, pd))

    @staticmethod
    def pd_swerling1(nfa, n_P, snrbar):
        snrbar = 10.0 ** (snrbar / 10.0)
        eps = 0.00000001
        delmax = 0.00001
        delta = 10000.0
        
        # Calculate the threshold Vt
        pfa = n_P * np.log(2) / nfa
        sqrtpfa = np.sqrt(-np.log10(pfa))
        sqrtnp = np.sqrt(n_P)
        vt0 = n_P - sqrtnp + 2.3 * sqrtpfa * (sqrtpfa + sqrtnp - 1.0)
        vt = vt0
        
        while abs(delta) >= vt0:
            igf = RadarDetection.incomplete_gamma(vt0, n_P)
            num = 0.5**(n_P/nfa) - igf
            temp = (n_P-1)*math.log(vt0+eps)-vt0-RadarDetection.factor(n_P-1)
            deno = np.exp(temp)
            vt = vt0 + (num / (deno+eps))
            delta = abs(vt-vt0)* 10000.0
            vt0 = vt

        # while delta >= delmax:
        #     igf = gammainc(n_P, vt0)  # Lower incomplete gamma (regularized)
        #     num = 0.5 ** (n_P / nfa) - igf
            
        #     # Handle factorial using gamma function (gamma(n) = factorial(n-1))
        #     if n_P - 1 < 0:
        #         deno = 0
        #     else:
        #         log_deno = (n_P - 1) * np.log(vt0 + eps) - vt0 - math.lgamma(n_P)
        #         deno = np.exp(log_deno)
            
        #     vt = vt0 + num / (deno + eps)
        #     delta = np.abs(vt - vt0)
        #     vt0 = vt
        
        if n_P==1:
            temp = -vt / (1.0 + snrbar)
            pd = np.exp(temp)
            return pd
        
        temp1 = 1.0 + n_P * snrbar
        temp2 = 1.0 / (n_P * snrbar)
        temp_val = 1.0 + temp2
        val1 = temp_val ** (n_P - 1.0)
        
        igf1 = RadarDetection.incomplete_gamma(vt, n_P - 1)  # For (np-1, vt)
        igf2 = RadarDetection.incomplete_gamma(vt / temp_val, n_P - 1)  # For (np-1, vt/temp_val)
        
        pd = 1.0 - igf1 + val1 * igf2 * np.exp(-vt / temp1)
        return pd
    
    @staticmethod
    def pd_swerling2(nfa, n_P, snrbar):
        snrbar = 10.0**(snrbar/10.0)
        eps = 0.00000001
        delmax = .00001
        delta =10000.0
        # Calculate the threshold V_T
        pfa = n_P * math.log(2) / nfa
        sqrtpfa = math.sqrt(-math.log10(pfa))
        sqrtnp = math.sqrt(n_P)
        vt0 = n_P - sqrtnp + 2.3*sqrtpfa*(sqrtpfa+sqrtnp-1.0)
        vt = vt0
        while abs(delta) >= vt0:
            igf = RadarDetection.incomplete_gamma(vt0, n_P)
            num = 0.5**(n_P/nfa)-igf
            temp = (n_P-1)*math.log(vt0+eps)-vt0-RadarDetection.factor(n_P-1)
            deno = math.exp(temp)
            vt = vt0 + (num / (deno+eps))
            delta = abs(vt-vt0)*10000.0
            vt0 = vt
        if n_P <= 50:
            temp = vt / (1.0 + snrbar)
            pd = 1.0 - RadarDetection.incomplete_gamma(temp, n_P)
        else:
            temp1 = snrbar + 1.0
            omegabar = math.sqrt(n_P) * temp1
            c3 = -1.0/math.sqrt(9.0*n_P)
            c4 = 0.25/n_P
            c6 = c3*c3/2.0
            V = (vt-n_P*temp1)/omegabar
            Vsqr = V*V
            val1 = math.exp(-Vsqr/2.0) / math.sqrt(2.0*np.pi)
            val2 = c3*(V**2-1.0) + c4*V*(3.0-V**2) - c6*V*(V**4-10.0*V**2 + 15.0)
            q = 0.5 * erfc(V/math.sqrt(2.0))
            pd = q - val1 * val2
        return pd
    
    @staticmethod
    def pd_swerling3(nfa, n_P, snrbar):
        snrbar = 10.0**(snrbar/10.0)
        eps = 0.00000001
        delmax = 0.00001
        delta =10000.0
        # Calculate the threshold Vt
        pfa = n_P * math.log(2)/nfa
        sqrtpfa = math.sqrt(-math.log10(pfa))
        sqrtnp = math.sqrt(n_P)
        vt0 = n_P - sqrtnp + 2.3*sqrtpfa*(sqrtpfa+sqrtnp-1.0)
        vt = vt0
        while abs(delta) >= vt0:
            igf = RadarDetection.incomplete_gamma(vt0, n_P)
            num = 0.5**(n_P/nfa) - igf
            temp = (n_P-1)*math.log(vt0+eps)-vt0-RadarDetection.factor(n_P-1)
            deno = math.exp(temp)
            vt = vt0 + (num / (deno+eps))
            delta = abs(vt-vt0)*10000.0
            vt0 = vt
        temp1 = vt / (1.0 + 0.5*n_P*snrbar)
        temp2 = 1.0 + 2.0/(n_P*snrbar)
        temp3 = 2.0*(n_P-2.0)/(n_P*snrbar)
        ko = math.exp(-temp1)* temp2**(n_P-2.0) * (1.0+temp1-temp3)
        if n_P <= 2:
            pd = ko
        else:
            temp4 = vt**(n_P-1.0) * math.exp(-vt)/(temp1 * math.exp(RadarDetection.factor(n_P-2.0)))
            temp5 = vt / (1.0+2.0/(n_P*snrbar))
            pd = temp4 + 1.0 - RadarDetection.incomplete_gamma(vt, n_P-1) + ko*RadarDetection.incomplete_gamma(temp5, n_P-1.0)
        return pd
    
    @staticmethod
    def pd_swerling4(nfa, n_P, snrbar):
        snrbar = 10.0**(snrbar/10.0)
        eps = 0.00000001
        delmax = 0.00001
        delta =10000.0
        # Calculate the threshold Vt
        pfa = n_P * math.log(2) / nfa
        sqrtpfa = math.sqrt(-math.log10(pfa))
        sqrtnp = math.sqrt(n_P)
        vt0 = n_P - sqrtnp + 2.3 * sqrtpfa * (sqrtpfa + sqrtnp - 1.0)
        vt = vt0
        while abs(delta) >= vt0:
            igf = RadarDetection.incomplete_gamma(vt0,n_P)
            num = 0.5**(n_P/nfa) - igf
            temp = (n_P-1) * math.log(vt0+eps) - vt0 - RadarDetection.factor(n_P-1)
            deno = math.exp(temp)
            vt = vt0 + (num / (deno+eps))
            delta = abs(vt - vt0) * 10000.0
            vt0 = vt
        h8 = snrbar /2.0
        beta = 1.0 + h8
        beta2 = 2.0 * beta**2 - 1.0
        beta3 = 2.0 * beta**3
        if n_P >= 50:
            temp1 = 2.0 * beta -1
            omegabar = math.sqrt(n_P * temp1)
            c3 = (beta3 - 1.) / 3.0 / beta2 / omegabar
            c4 = (beta3 * beta3 - 1.0) / 4. / n_P /beta2 /beta2
            c6 = c3 * c3 /2.0
            V = (vt - n_P * (1.0 + snrbar)) / omegabar
            Vsqr = V *V
            val1 = math.exp(-Vsqr / 2.0) / math.sqrt( 2.0 * np.pi)
            val2 = c3 * (V**2 -1.0) + c4 * V * (3.0 - V**2) - c6 * V * (V**4 - 10. * V**2 + 15.0)
            q = 0.5 * erfc (V/math.sqrt(2.0))
            pd = q - val1 * val2
            return pd
        else:
            snr = 1.0
            gamma0 = RadarDetection.incomplete_gamma(vt/beta,n_P)
            a1 = (vt / beta)**n_P / (math.exp(RadarDetection.factor(n_P)) * math.exp(vt/beta))
            sum = gamma0
            for i in range(1, n_P+1, 1):
                temp1 = 1
                if i==1:
                    ai = a1
                else:
                    ai = (vt / beta) * a1 / (n_P + i -1)
                a1 = ai
                gammai = gamma0 - ai
                gamma0 = gammai
                a1 = ai
                for ii in range(1, i+1, 1):
                    temp1 = temp1 * (n_P + 1 - ii)
                term = (snrbar /2.0)**i * gammai * temp1 / math.exp(RadarDetection.factor(i))
                sum = sum + term
            pd = 1.0 - sum / beta**n_P
        pd = max(pd, 0.0)
        return pd
    
    @staticmethod
    def fluct_loss(pd, pfa, n_P, sw_case):
        '''
        This function calculates the SNR fluctuation loss for Swerling models,
        A negative Lf value indicates SNR gain instead of loss
        '''
        # compute the false alarm number
        nfa = math.log(2) / pfa
        PD_SW = 0.0
        SNR_SW = 0.0
        # *************** Swerling 5 case ****************
        # check to make sure that np>1
        if n_P == 1:
            b = math.sqrt(-2.0 * math.log(pfa))
            Pd_Sw5 = 0.001
            snr_inc = 0.1 - 0.005
            while Pd_Sw5 <= pd:
                snr_inc = snr_inc + 0.005
                a = math.sqrt(2.0 * 10**(.1*snr_inc))
                Pd_Sw5 = RadarDetection.marcumsq_new(a,b)
            PD_SW5 = Pd_Sw5
            SNR_SW5 = snr_inc
        else:
            # np > 1 use MATLAB function pd_swerling5.m
            snr_inc = 0.1 - 0.005
            Pd_Sw5 = 0.001
            while Pd_Sw5 <= pd:
                snr_inc = snr_inc + 0.005
                Pd_Sw5 = RadarDetection.pd_swerling5(pfa, 1, n_P, snr_inc)
            PD_SW5 = Pd_Sw5
            SNR_SW5 = snr_inc
        if sw_case == 5:
            Lf = 0.0
            return Lf, PD_SW5, SNR_SW5, PD_SW5, SNR_SW5
        # *************** Swerling 5 case ****************
        # *************** Swerling 1 case ****************
        if sw_case == 1:
            Pd_Sw1 = 0.001
            snr_inc = 0.1 - 0.005
            while Pd_Sw1 <= pd:
                snr_inc = snr_inc + 0.005
                Pd_Sw1 = RadarDetection.pd_swerling1(nfa, n_P, snr_inc)
            PD_SW1 = Pd_Sw1
            SNR_SW1 = snr_inc
            Lf = SNR_SW1 - SNR_SW5
            PD_SW, SNR_SW = PD_SW1, SNR_SW1
        # *************** End Swerling 1 case ************
        # *************** Swerling 2 case ****************
        if sw_case == 2:
            Pd_Sw2 = 0.001
            snr_inc = 0.1 - 0.005
            while Pd_Sw2 <= pd:
                snr_inc = snr_inc + 0.005
                Pd_Sw2 = RadarDetection.pd_swerling2(nfa, n_P, snr_inc)
            PD_SW2 = Pd_Sw2
            SNR_SW2 = snr_inc
            Lf = SNR_SW2 - SNR_SW5
            PD_SW, SNR_SW = PD_SW2, SNR_SW2
        # *************** End Swerling 2 case ************
        # *************** Swerling 3 case ****************
        if sw_case == 3:
            Pd_Sw3 = 0.001
            snr_inc = 0.1 - 0.005
            while Pd_Sw3 <= pd:
                snr_inc = snr_inc + 0.005
                Pd_Sw3 = RadarDetection.pd_swerling3(nfa, n_P, snr_inc)
            PD_SW3 = Pd_Sw3
            SNR_SW3 = snr_inc
            Lf = SNR_SW3 - SNR_SW5
            PD_SW, SNR_SW = PD_SW3, SNR_SW3
        # *************** End Swerling 3 case ************
        # *************** Swerling 4 case ****************
        if sw_case == 4:
            Pd_Sw4 = 0.001
            snr_inc = 0.1 - 0.005
            while Pd_Sw4 <= pd:
                snr_inc = snr_inc + 0.005
                Pd_Sw4 = RadarDetection.pd_swerling4(nfa, n_P, snr_inc)
            PD_SW4 = Pd_Sw4
            SNR_SW4 = snr_inc
            Lf = SNR_SW4 - SNR_SW5
            PD_SW, SNR_SW = PD_SW4, SNR_SW4
        # *************** End Swerling 4 case ************
        return Lf, PD_SW5, SNR_SW5, PD_SW, SNR_SW
    

